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This  report  develops  the  algorithm  for  deriving  attitude,  heading,  and  navigation 
information  from  a  strapdown  inertial  system.  Beginning  with'the  fundamental  physical 
relationships,  it  develops  all  required  equations  and  progresses  to  the  onboard 
implementation  of  the  algorithm^ 

Significant  features  of  the  algorithm  Include:  (1)  Computations  performed  in  the 
wander  azimuth  coordinate  frame  to  provide  a  system  capable  of  operating  in  the  polar 
regions;  (2)  Separation  into  four  loops  of  different  iteration  rates.  This  maintains 
rapid,  accurate  updating  of  the  direction  cosine  matrix  involving  vehicle  attitude, 
while  processing  other  information  and  extracting  display  data  at  appropriately  slower 
rates;  (3)  Fourth  order  Runge-Kutta  integration  of  quaternions,  using  second  order 
rate  extraction,  to  update  the  attitude  direction  cosine  matrix;  (4)  Specification  of 
the  computations  that  require  double  precision  for  adequate  performance;  (5)  Third 
order  damping  of  the  vertical  channel  by  means  of  barometric  altimeter  data.  ' 

The  applicability  of  this  algorithm  to  a  range  of  vehicle  and  mission  environments 
is  indicated,  the  required  adaptations  being  easily  performed  for  each  particular 
implementation. 
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ABSTRACT 


Thisreport  develops  the  algorithm  for  deriving  attitude,  heading, 
and  navigation  information  from  a  strapdown  inertial  system.  Beginning 
with  the  fundamental  physical  relationships,  it  develops  all  required 
.equations  and  progresses  to  the  onboard  implementation  of  the  algorithm. 

Significant  features  of  the  algorithm  include: 

(1)  Computations  performed  in  the  wander  azimuth  coordinate  frame  to 
provide  a  system  capable  of  operating  in  the  polar  regions; 

(2)  Separation  into  four  loops  of  different  iteration  rates.  This 
maintains  rapid,  accurate  updating  of  the  direction  cosine  matrix 
Involving  vehicle  attitude,  while  processing  other  Information  and 
extracting  display  data  at  appropriately  slower  rates; 

(3)  Fourth  order  Runge-Kutta  integration  of  quaternions,  using 
second  order  rate  extraction,  to  update  the  attitude  direction  cosine 
matrix; 

(4)  Specification  of  the  computations  that  require  double  precision 
for  adequate  performance; 

(5)  Third  order  damping  of  the  vertical  channel  by  means  of 
barometric  altimeter  data. 

The  applicability  of  this  algorithm  to  a  range  of  vehicle  and  mission 
environments  is  Indicated,  the  required  adaptations  being  easily  performed 
for  each  particular  Implementation. 
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SECTION  I 
INTRODUCTION 

In  a  gimballed  inertial  system,  the  gyros  and  accelerometers  are 
located  on  a  platform  whose  orientation  with  respect  to  the  vehicle  is 
determined  by  a  set  of  gimbals.  The  gyro  outputs  are  used  to  drive 
gimbal  motors  in  such  a  way  as  to  maintain  the  platform  in  alignment 
with  some  desired  coordinate  frame,  regardless  of  the  vehicle  orientation 
in  space.  Thus,  the  accelerometer  outputs  are  coordinatized  in  that 
desired  frame,  and  navigation  information  can  easily  be  generated  by 
performing  computations  in  the  instrumented  coordinate  system.  Further¬ 
more,  the  vehicle  orientation  with  respect  to  that  coordinate  system  can 
be  determined  by  observing  the  angles  formed  between  the  various  gimbals. 

On  the  other  hand,  the  gyros  and  accelerometers  of  a  strapdown  system 
are  attached  to  the  vehicle  body,  and  therefore  provide  signals  propor¬ 
tioned  to  specific  force  and  angular  rate  between  the  inertial  and  body 
frames,  coordinatized  in  the  body  frame.  The  inertial  measurement  unit 
itself  is  significantly  simpler  than  a  gimballed  system,  allowing  easier 
maintenance  and  part  replacement,  while  not  being  limited  in  performance 
by  the  imperfect  response  of  the  gimballing  mechanization.  Furthermore, 
instrument  redundancy  is  more  readily  accommodated  in  a  strapdown  con¬ 
figuration.  However,  the  simplicity  of  the  strapdown  system  is  gained 
at  the  expense  of  subjecting  the  gyros  and  accelerometers  to  a  more 
severe  environment,  that  of  the  vehicle  itself.  Furthermore,  the  com¬ 
putational  load  Is  greater,  since  the  onboard  computer  must  maintain  an1 
analytical  representation  of  the  desired  coordinate  frame,  in  addition 
to  the  calculations  it  would  perform  with  a  gimballed  system. 

This  report  develops  the  algorithm  for  deriving  attitude,  heading, 
and  navigation  information  from  a  strapdown  system.  It  will  be  imple¬ 
mented  in  an  onboard  computer  during  a  strapdown  development  and 
evaluation  project  sponsored  jointly  by  the  Air  Force  Flight  Dynamics 
Laboratory  and  the  Army  Electronics  Command. 
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Significant  features  of  the  algorithm  include: 

(1)  Computations  performed  in  the  wander  azimuth  coordinate  frame  to 
provide  a  system  capable  of  operating  in  the  polar  regions; 

(2)  Separation  into  four  loops  of  different  iteration  rates.  This 
maintains  rapid,  accurate  updating  of  the  direction  cosine  matrix 
involving  vehicle  attitude,  while  processing  other  information  and 
extracting  display  data  at  appropriately  slower  rates; 

(3)  Fourth  order  Runge-Kutta  integration  of  quaternions,  using 
second  order  rate  extraction,  to  update  the  attitude  direction  cosine 
matrix; 

(4)  Specification  of  the  computations  that  require  double  precision 
for  adequate  performance; 

(5)  Third  order  damping  of  the  vertical  channel  by  means  of  baro¬ 
metric  altimeter  data. 

The  report  is  arranged  in  the  following  manner.  Section  II  introduces 
the  appropriate  notation  and  coordinate  frames,  and  then  it  describes  the 
relationships  among  coordinate  frames  by  means  of  direction  cosine 
matrices  and  Euler  angles.  Furthermore,  the  time  propagation  of  the 
direction  cosine  matrix  is  described  in  terms  of  differential  equations 
for  the  matrix  entries  and  equivalently  in  terms  of  quaternions.  Section 
III  presents  a  conceptual  description  of  the  algorithm  and  then  develops 
the  details  of  its  various  segments.  Section  IV  considers  the  aspects  of 
onboard  implementation,  as  integration  methods,  processing  rates,  and 
rate  extraction  from  gyro  output  pulse  counters.  The  final  algorithm  is 
also  exhibited  in  the  form  of  scalar  equations.  In  conclusion,  Section  V 
Indicates  the  applicability  of  this  algorithm  to  a  range  of  vehicle  and 
mission  environments. 
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SECTION  II 
FUNDAMENTALS 


1.  NOTATION 

A  brief  description  of  the  notation  to  be  employed  is  presented  in 
this  section.  It  may  appear  somewhat  cumbersome  at  first,  but  the 
clarity  it  lends  to  the  technical  developments  warrants  its  use  [Ref¬ 
erences  2,  11]. 

Certain  letters  will  be  used  as  subscripts  or  superscripts  to  specify 
particular  coordinate  frames.  These  are: 

i  =  inertial;  with  origin  at  the  earth's  center  and  nonrotating  with 
respect  to  inertial  space 

e  =  earth;  origin  at  the  earth's  center  and  nonrotating  with  respect 
to  the  earth;  the  unit  vectors  point  toward  the  equator  and  the 
Greenwich  meridian  (xj,  equator  and  90°E  (yj,  and  the  North 
Pole  (ze).  e  e 

n  =  north-east-down;  centered  at  the  vehicle  CG  and  pointing  north 
(x„),  east  <yn),  and  down  (zn). 

p  =  "platform";  wander  azimuth  coordinate  system  centered  at  the 
vehicle  CG  and  displaced  from  the  n  frame-by  an  angle  (-  o)  about 
zn,  where  the  usual  convention  is  used:  that  positive  angles  are 

defined  as  clockwise  as  one  looks  in- the- direction  of  the  axis 
about  which  the  rotation  is  made. 

b  =  body;  centered  at  the  vehicle  CG,  with  xfc  along  the  longitudinal 
axis,  yb  out  the  right  side,  and  zb  out  the  underside  of  the 
vehicle. 

Vectors  are  represented  by  an  underscored  lower  case  letter,  and  a 
superscript  denotes  the  coordinate  frame  in  which  the  vector  is  expressed 
mathematically.  For  example,  is  the  specific  force  vector  expressed 
as  a  three-dimensional  vector  quantity  coordinatized  in  the  body  (vehicle) 
frame.  Angular  velocity  vectors  will  additionally  have  two  subscripts, 


I 


-I 

3 


•1 

1 
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denoting  the  two  reference  frames  between  which  the  angular  velocity 
exists.  The  quantity  m  !?e  is  the  angular  velocity  from  the  inertial 
frame  to  the  earth  frame,  represented  as  a  three-dimensional  vector 
coordinatized  in  the  body  frame. 

Components  of  vector  quantities  are  denoted  by  the  same  lower  case 

letter,  without  underscoring  but  with  a  subscript  x,  y,  or  z.  Thus,  f*| 
b  * 

and  Q  are  the  body  frame  x-components  (longitudinal)  of  f  and  a 

respectively. 


The  three-by-three  direction  cosine  matrix  that  transforms  a  vector 
from  one  coordinate  frame  to  another  is  denoted  by  an  underscored  capital 
C,  with  subscript  denoting  the  original  frame  and  superscript  for  the 
new  frame.  For  instance,  C  £  is  the  matrix  that  transforms  a  vector  from 
the  b  frame  to  the  1  frame,  as 


For  convenience,  the  elements  of  this  matrix  are  normally  written  without 
the  subscript  and  superscript  once  the  matrix  itself  is  delineated. 


Equation  1  thus  becomes 


(2) 


Here  the  subscripts  (1,  2,  3)  and  (x,  y,  z)  are  interchangeable.  Each 
element  of  £  ^  is  a  direction  cosine  between  the  unit  vectors  of  the  b 
and  i  frames.  For  instance,  is  the  direction  cosine  between  the  x 
(or  1)  axis  of  the  i  frame  and  the  y  (or  2)  axis  of  the  b  frame. 
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The  values  of  the  entries  of  a  direction  cosine  matrix  can  be 

specified  completely  by  their  initial  conditions  and  knowledge  of  the 

angular  rates  between  the  two  frames  of  interest.  For  instance,  C  ?  (t) 
b  “  d 

(or  C  ,  (t)  )  is  specified  by  an  initial  condition  and  knowledge  of 

either  a  bb  or  u  jb,  the  angular  rate  from  the  i  frame  to  the  b  frame, 

coordinatized  in  either  frame.  (Note  that  “  ^  =  *  $£  >  and  similarly 

—  1b  *  '  -  bi  *n  sVstem  diagrams,  this  dependence  will  be  represented 

as  in  Figure  1.  The  angular  velocity  u  bb  used  to  propagate  the  direction 

cosine  matrix  enters  the  block  labelled  C  b  from  above  or  below.  Vectors 
that  are  to  be  transformed  by  the  matrix  enter  the  block  from  the  side. 


Writing  out  the  propagation  relations  explicitly  will  employ  a  ’'cross- 
matrix''.  This  matrix  is  defined  to  be  the  three-by-three  matrix  which, 
when  postmultiplied  by  a  vector  coordinatized  in  the  appropriate  frame, 
yields  the  result  of  performing  a  cross-product  on  that  vector  in  those 
coordinates.  Let  rb  be  some  vector  in  the  b  frame,  and  suppose  one 
desires  to  compute  (u  ib  x  r  )b;  the  matrix  W  b£  is  defined  to  satisfy 

wbblb-  (a.bXl)1’  (4) 

Thus,  W  bb  is  defined  as 


\iibK  _ 

—ib  - 


'  b  b 

O  -U),bjW,by 

wibz  0 

|-wbt,y  wbb,  0 


t 

/  b 


where  the  entries  are  the  components  of  the  vecto'  u  bfc 


(5) 


Time  derivatives  of  coordinatized  vector  quantities  will  be  denoted 
by  a  dot  above  the  quantity  or  the  operator ,p  preceding  the  quantity: 

^  ivb=  (6) 

With  physical  vectors  (for  Instance  x,  as  opposed  to  xb  which  is  the 
set  of  three  scalars  that  defines  x  in  terms  of  body  coordinates),  the 


1 

/ 
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operator  p  will  bear  a  subscript  to  depict  the  coordinate  frame  used 
to  observe  the  rate  of  change  of  the  vector  In  question.  With  this 
notation,  the  Theorem  of  Coriolis  becomes,  for  any  vector  v  , 

Pi  v  =  pbv  +u>jbx  y  (7) 

2.  COORDINATE  L'YSTEM  RELATIONSHIPS:  DIRECTION  COSINES 

AND  EULER  ANGLES 

The  coordinate  systems  of  particular  Interest  for  this  report  are 
the  body  (b),  wander  azimuth  or  "platform"  (p),  and  earth  (e)  frames. 

First  of  all,  the  strapdown  instruments  provide  Information  coordinatized 
in  the  body  frame,  while  algorithm  computations  will  be  conducted  in  the 
wander  azimuth  frame.  Further,  the  relationship  between  these  two  frames 
will  specify  the  aircraft  attitude.  Finally,  the  relationship  between 
the  wander  azimuth  frame  and  the  earth  frame  will  determine  the  location 
of  the  vehicle. 

There  are  two  familiar  means  of  describing  rotations  of  one  coordinate 
frame  relative  to  another.  Euler  angles  are  a  set  of  three  rotations 
taken  in  a  specified  order  to  generate  the  desired  orientation.  The  same 
orientation  can  be  uniquely  described  by  means  of  the  nine  direction 
cosines  that  exist  between  the  unit  vectors  of  the  two  frames.  Whereas 
there  is  no  redundancy  in  the  Euler  angle  description,  there  are  six 
constraints  on  the  direction  cosines  (the  sums  of  the  squares  of  elements 
in  a  single  row  or  column  equal  unity).  Despite  this  redundancy,  the 
direction  cosine  technique  is  preferable  because  the  Euler  angle  de¬ 
scription  has  an  Indeterminate  orientation  (analytical  "gimbal  lock") 
and  a  surrounding  region  of  poor  resolution. 

The  final  algorithm  in  this  report  will  employ  two  direction  cosine 
matrices,  one  propagated  by  conventional  means  and  the  other  evaluated  as 
a  function  of  an  appropriate  quaternion.  Essentially,  a  quaternion  is  a 
means  of  describing  angular  orientation  with  four  parameters,  the  minimum 
redundancy  that  removes  the  Indeterminate  points  of  three-parameter 
descriptions.  Although  the  loop  computations  use  direction  cosines,  the 
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data  extracted  for  display  or  autopilot  Inputs  is  most  conveniently  ex¬ 
pressed  in  the  form  of  Euler  angles.  Thus,  the  subsequent  sections  will 
delineate  the  calculation  of  Euler  angles  from  the  corresponding  direction 
cosine  matrices. 

a.  Hander  Azimuth  and  Earth  Frames 

Figure  2  portrays  the  relationship  between  the  earth  and  wander 
azimuth  frames.  When  the  three  angles,  A,  L  and  a,  are  ;»ro,  the  wander 
azimuth  f  -me  is  oriented  so  that  xp  is  aligned  with  zfi,  yp  with  yfi,  and 
zp  with  -xe-  A  positive  rotation  of  A,  the  longitude,  is  taken  about 
the  xp  axis.  Subsequently,  a  negative  rotation  about  the  displaced  yD 
axis,  of  magnitude  L  (latitude),  is  made.  Finally,  a  negative  rotation 
about  the  displaced  zp  axis  Is  made,  of  magnitude  a  (wander  azimuth 
angle).  Use  of  a  negative  rotation  was  chosen  arbitrarily  to  conform  to 
the  usual  definition  of  a  as  a  counterclockwise  rotation  as  seen  from 
above. 


From  the  figure  it  can  be  seen  that  geographic  latitude,  rather  than 
geocentric,  will  be  used  to  describe  position  on  the  earth.  Thus  zp  is 
normal  to  the  elliptical  surface  of  the  earth  rather  than  in  the  direction 
of  the  earth  center.  Throughout  the  development  of  this  report,  the 
ellipticlty  of  the  earth  w'll  be  accounted  for,  but  higher  order  effects 
and  local  geoid  perturbations  will  be  neglected. 


The  direction  cosine  matrix  C  £  can  be  developed  as  the  product  of 

Individual  rotation  matrices.  First  the  p  frame  is  rotated  to  align  the 

axes  as  x„  «  z.,  y„  *  y.,  and  z„  ■  -x„.  Then  rotations  of  A  about  x., 

P  e  p  e  p  e  p 

-l  abou>  yp,  and  -a  about  zp  are  perfonr^d  in  that  order.  Thus, 


cosa-sina  0 

cosLO  smL 

\  0  0 

O  0  1 

S 

sma  cos  a  0 

0  1  0 

0  cosX  smX 

0  1  0 

0  0  l 

smLOcosL 

0-smX  cosX 

-IOO 

(8) 
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The  subscript  p  denotes  principal  values  In  Equations  12  and  13. 

Latitude  Is  defined  In  the  range  (-90°,  90°),  which  coincides  with  the 
principal  values  of  the  sin"'  function,  and  thus  there  Is  no  ambiguity 
to  the  solution  of  Equation  11.  However,  X  Is  defined  over  (-180°,  180°) 
and  a  over  (0°,  360°),  whereas  the  principal  values  of  the  tan''  function 
lie  within  (-90°,  90°).  The  ambiguity  can  be  resolved  by  observing  the 
sign  of  the  elements  C31  and  C13‘  Since  cos  L  Is  nonnegative,  these 
elements  yield  the  sign  of  -cos  X  and  cos  a  respectively.  Table  I 
demonstrates  the  evaluation  of  the  true  X  and  a  values  as  a  function  of 
these  signs. 
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Using  conventional  propagation  of  the  direction  cosine  matrix,  it  is 
possible  to  neglect  the  propagation  of  C^,  C21>  and  Cji  if  these  values 
are  not  required  for  other  computations.  The  elements  and  C21  will 
not- be  used  at  all,  and  C31  is  required  only  to  compute  the  value  of 
longitude.  However,  by  manipulating  the  relations  of  Equation  10,  it  can 
be  shown  that 

C3I=C23CI2  ~Cj2C|j  04) 

Thus,  only  six  of  the  nine  direction  cosines  will  be  propagated,  and 
Equation  14  used  to  calculate  the  value  of  C31  for  the  longitude 
evaluation. 


b.  Hander  Azimuth  and  8ody  Frames 


Figure  3  presents  the  relationships  between  the  wander  azimuth  and 
body  frames.  Rotating  the  wander  azimuth  frame  through  a  positive 
rotation  of  a  about  zp  yields  a  north-east-down  coordinate  frame.  From 
there  the  ordinary  Euler  angles  of  yaw  (if),  pitch  (6),  and  roll  (<J)  yield 
the  aircraft  attitude.  Consequently,  the  matrix  Cp  can  be  composed  of 
the  product  of  three  rotation  matrices,  (<|>  +  a)  about  zb>  then  e  about 
the  displaced  yb,  and  $  about  x^: 


1  0  0 

cos  S 

0  -sm0 

coshR+a)  sin(f+a)  0 

rb  _ 
cp  - 

0  cos<£  sin</> 
0-$in  <f>  cos<#> 

0 

$m0 

1  0 

0  co$S 

-sm(«|/+a >  costy+a)  o 

0  0  1 

The  inverse  transformation  will  be  of  direct  Interest,  so  define 


rP  . 
£b  - 


Tll  f|Z  TI3 
T2i  T22T23 
■SI  Tj2  Tm 


=  c 


bT 

P 


(16) 
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NOTE  :  ORIGIN  OF  p  FRAME  OISPLACEO  FROM  THAT  OF  b  FFUME 
ONLY  FOR  CLARITY  OF  DIAGRAM  \  THEY  ARE  ACTUALLY 
COINCIDENT  AT  VEHICLE  C3. 


Figure  3.  Hander  Azimuth  ("Platform")  and  Body  Coordinate  Frames 
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Using  this  definition,  the  individual  elements  become 
Tu  =  cosi^z+a)  cos 9 

Ti2  *  cos(^  +a )  sin 9  sin <f>  -sm(i//+a)  cos<£ 

T,j  =  cos(<//  +  a)  sin#  cos<£  +sin(^+a)  sw<£ 

I2,  =  sin(i/z  +a)cos# 

sm(i/z +q)  $m 9  sm^>  +  cos{^+a)cos^>  (17) 

T23 =  sm  ( +q  )  sin  $  cos^>“cos(^+a)cos0 
T3,  •  -sin# 

1 32=  cos#  $in<£ 

T»«  cos 8  ccs  o 

The  values  of  (^  +  a),  8,  and  4  are  obtained  from  the  elements  of 

C  P  in  a  manner  similar  to  the  data  extraction  of  the  previous  section: 

“  b 


sin'l-Tj,) 

(18) 

d>p  =  lon‘l(TK/T33) 

(19) 

OP+a  )p=  ton'lTj./T,,) 

(20) 

Pitch  is  defined  over  the  range  (-90°,  90°)  and  no  ambiguities  exist. 
However,  roll  is  defined  over  (-180°,  180°)  and  (ip  +  a)  over  (0°,  360°). 
Cos  8  is  nonnegative,  so  the  ambiguity  can  be  resolved  with  the  aid  of 
the  sign  of  T33  and  V  as  in  Table  II. 

Once  the  value  of  (ip  +  a)  is  obtained,  it  can  be  combined  with  the 
value  of  a  derived  in  the  previous  section  to  obtain  yaw: 

'P  =  ('P+al  -  a  (21) 

If  the  result  is  a  negative  number,  360°  can  be  added  to  it  to  yield  a 
value  of  <p  in  the  range  of  (0°,  360°). 

3.  DIRECTION  COSINE  RATES 

In  order  to  propagate  the  values  of  the  direction  cosine  matrix 
elements  in  the  conventional  manner,  an  expression  for  their  rate  of 
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TABLE  II 

ROLL  AND  YAW-PLUS-WANDER  AZIMUTH  EVALUATIONS 
ROLL  ($) 


Sign  of  $p 

Sign  of  T33 

$  Evaluation 

Quadrant 

+ 

- 

yl80“ 

(-180“,  -90°; 

- 

+ 

*P 

1 

to 

o 

o 

o 

o 

+ 

+ 

*P 

(0“,  90“) 

” 

*p  +  180“ 

YAW-PLUS-WANDER  AZIMUTH  +  a) 

(90“,  180°) 

Sign  of  OF  +  a)p 

Sign  of 

(*  +  «)p 
Evaluation 

Quadrant 

+ 

+ 

(V>  +  “)p 

(0“,  90°) 

- 

- 

(1-  +  o)p  +  180“ 

(90“,  180“) 

+ 

- 

(V  +  ct)p  +  180“ 

(180“ ,  270°) 

- 

+ 

(V  +  ct)p  +  360“ 

(270“ ,  360“) 

■> 

I 

$ 

1 
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If  the  available  angular  rate  information  were  coordinatized  in 
the  a  frame  instead  of  the  b  frame,  the  desired  relations  become 

~a  .  wak  -a  (28) 

£b  -  *ob  2b 


a  Jf 

where  W  ^  is  defined  as  in  Equation  26,  except  that  the  entries  are 
components  of  u  ab  (=  -a  ba).  To  show  this,  write  Equation  25  with  the 
roles  of  a  and  b  reversed: 


ib  .  «b  ^.ok 
20  -  So  “ba 


Now  use  the  facts  that  C  a  =  C  j^T  and  a  ba  =  -u  ab  to  write 


{<£)  ■  (C0bf  =  2bT  <1  '  ~  -bT  -ob 


Transposing  yields 


ao  .  y.ak  T  -a 
2b  -  -  *ob  Sb 


=  Wak  C? 
~ab  “b 


which  is  Equation  28. 


It  has  been  common  practice  to  solve  these  equations  with  a  first 
order  integration  technique  using  OOA’s  to  achieve  high  iteration  rates 
(on  the  order  of  10,000  per  second)  and  thereby  maintain  accuracy.  The 
current  algorithm  is  designed  for  whole  word  computers,  using  a  slower 
iteration  rate  but  a  higher  order  integration  method  where  necessary  for 
precision.  Because  of  the  amount  of  computation  involved  in  these  higher 
order  methods,  it  is  more  efficient  to  propagate  four  quaternion  par¬ 
ameters  instead  of  nine  direction  cosines,  and  then  calculate  the  matrix 
entries  algebraically. 


4.  QUATERNION  UPDATING  OF  DIRECTION  COSINE  MATRICES 

As  mentioned  previously,  a  rotation  of  one  frame  relative  to  another 
can  be  expressed  uniquely  in  terms  of  four  parameters,  three  to  define 
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I 

the  axis  of  rotation  and  the  fourth  to  specify  the  amount  of  rotation. 

A  considerable  amount  of  quaternion  algebra  can  be  developed  to  provide 
these  expressions  [References  5,  7,  9],  but  it  is  more  convenient  for  the 
purpose  of  this  algorithm  to  define  the  direction  cosine  matrix  in 
terms  of  the  propagated  quaternions  [References  6,  7,  10].  Coordinate 
transformations  are  then  performed  in  the  conventional  manner. 

Let  A,  8,  C,  and  D  be  the  four  quaternion  parameters.  Conceptually, 
A,  B,  and  C  will  define  a  vector  in  space  and  D  will  be  the  amount  of 
rotation  about  that  vector.  With  appropriate  definitions  of  these  four 
parameters,  a  general  direction  cosine  matrix,  C  ^  as  in  the  previous 
section,  can  be  evaluated  as 

A2-B2-C2+D2  2(AB  +  C0)  2(AC-BD) 

C°  =  2(AB-CD)  -A2  +  82-C2  +  D2  2(BC  +  AD) 

”  b 

2(AC  +  BD)  2(8C  -AD)  -A2  -  B2  +  C2  +  D2 

Note  that  changing  the  sign  of  D  yields  C  j^T  =>  C  This  is  expected 
since  the  negative  of  the  rotation  about  the  unit  vector  used  to  obtain 
frame  a  should  yield  the  original  frame  b  again.  Furthermore,  changing 
the  sign  of  all  four  quaternion  parameters  leaves  C  ^  unchanged.  This, 
too,  is  reasonable:  a  positive  rotation  about  a  certain  vector  equals 
the  negative  rotation  of  same  magnitude  about  the  vector  in  the  opposite 
direction.  This  fact  will  be  utilized  in  the  initialization  of  the 
quaternions. 


Now  a  rate  equation  analogous  to  Equation  25  will  be  developed. 
Given  the  definition  Equation  29  and  the  angular  rate  information 
-  ab  =  ba’  ttl8  aPPr°Pr’ate  rate  equation  can  be  derived  as 
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/ 


If  instead  the  angular  rate  data  were  available  in  a-frame/' 
coordinates,  the  relation  is  / 


A 

B 

1 

“  2 

C 

6 

0  -w. 


0 


(32) 


where 


w°h 

-ab 


o 


(33) 


Note  that  Equation  30  or  32  would  also  be  satisfied  if  the  sign  of  all 
four  parameters  were  reversed;  again  this  is  useful  for  initialization 
considerations. 


Since  there  is  a  single  redundancy  in  a  four  parameter  description 
of  coordinate  rotations  (as  opposed  to  six  for  the  direction  cosines), 
the  quaternion  parameters  satisfy  a  single  constraint  equation: 

A2  +  B2  +  C2  +  D2  =  I  (34) 

2  2  2  2  1/2 

Computationally,  (A  +  B  +  C  +  0 )  '  can  be  used  as  a  normalizing 
factor  for  each  parameter;  this  is  analogous  to  the  periodic  orthogonal- 
ization  procedure  used  in  conjunction  with  direction  cosine  propagations. 
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Quaternions  will  be  employed  to  update  C  [J.  Thu  b  frame  is  obtained 
from  the  p  frame  by  a  rotation  of  (¥  +  a)  about  zb,  6  about  yb,  and 
finally  $  about  xb-  An  appropriate  definition  of  A,  B,  C,  and  D  that 

yields  C  £  accordino  to  Eauation  29  and  also  satisfies  Eouation  34  is: 

8_  ■  A 

(35a) 
(35b) 
(35c) 
(35d) 


\i/  +a  Q  d> 

A  =  sin  —  sin  —  cos  — 


B  =-cos 


2 

\p  +a 


B 


2 

* 


C  =  -sin  cos 


sin  cos  y 

4> 


-  sin 


+  a  g 
D  =  cos  2  -  cos  — 


sin 


2 

i)/+a 

2 

i^/+a 

2 

<l/+a 


cos  y  sin  y 

Q 

cos  y  sin  J- 


sin  y  sin  y 


g  .  <t> 

sin  y  sin  y 


Although  these  relations  define  the  quaternion  parameters,  they  are 
not  convenient  for  1 'itialization.  Besidps  requiring  sine  and  cosine 
evaluations,  the  triple  products  of  those  functions  suffer  from  potential 
numerical  inaccuracies  on  a  short  wordlength  computer.  Moreover,  typical 
Initialization  procedures  yield  the  direction  cosines  directly,  with 
Euler  angles  computed  from  these.  Thus,  the  initial  magnitudes  of  A,  B, 
C,  and  D  can  be  defined  more  appropriately  In  :erms  of  elements  of  C  b: 


!  ; 


lAl  =  rg  +  T„  -  T22  -  Tjj 
•S'  =  iy>~  T„+Tm-T„ 


ICl  y  ^/|  -  T|,  -  T2Z  +  Tjj 


IdI  =  yj  I  -  A*  —  Bz  -  C* 


(36a) 

(36b) 

(36c) 

(36d) 
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The  first  three  result  from  algebraic  manipulation  of  the  diagonal  terms 
of  Equation  29,  and  the  last  is  just  a  restatement  of  the  constraint 


Equation  34.  The  signs  of  these  terms  need  to  be  established.  Taking 
the  difference  of  off-diagonal  terms  in  Equation  29  yields 

4AD  =  T„  -  Tj2  (37a) 

4B0  =  TSI  -  Tjj  (37b) 

4CD  -  Tt2  -  T2|  (37c) 

Thus,  once  the  sign  of  D  is  set,  these  three  relations  provide  the  signs 

of  the  other  three  parameters.  It  was  previously  demonstrated  that  the 
choice  is,  in  fact,  arbitrary.  Therefore,  the  signs  are  chosen  as 

sign  D  =  +  (38a) 

sign  A  =  sign  (T2J  -  T32)  (38b) 

sign  B  =  sign  (Tsl  -  T|S)  (38c) 

sign  C  *  sign  (T,2  -  T2|)  (38d) 


At  initialization  time.  Equations  36  and  38  are  used  to  evaluate 
A,  B,  C,  and  D  from  the  value  of  C  £  established  in  alignment.  In  the 

operational  mode,  u  is  available  so  the  parameters  are  updated 

according  to  Equation  30,  with  Equation  31  defined  as  a  j^.  When  C  ^  is 
required,  it  is  evaluated  by  means  of  Equation  29. 
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SECTION  III 

DEVELOPMENT  OF  ALGORITHM 


1.  CONCEPTUAL  DESCRIPTION  OF  SYSTEM 

The  basic  computational  system  structure  is  as  depicted  in  Figure  4. 
Body  mounted  accelerometers  measure  a*3,  the  specific  force  exerted  on  the 
instruments  by  their  supports  {vehicle  acceleration  minus  the  acceleration 
of  mass  attraction  gravity)  along  body  coordinates.  This  is  simply  the 
negative  of  fb,  conventionally  defined  as  the  specific  force  exerted  by 
the  Instrument  set  on  its  supports.  Similarly,  gyros  provide  a  measure 
of  the  angular  rate  between  inertial  space  and  the  body  frame,  expressed 
as  components  along  body  axes. 


First  the  specific  force  a°  is  transformed  by  the  direction  cosine 


matrix  C  {j  into  wander  azimuth  coordinates: 

op  =  Cpob 


A  current  value  for  C  jj  is  maintained  through  propagations  using  w 
which  is  generated  as 


(39) 
b 


b  b  b 
<u  ,  -  u  -  w. 

— pb  “lb  “Ip 

=  <ub  -Cb  <uP 
-lb  "P  "Ip 

b  .pT  p 
=  w,k  —  cr  wr 
lb  -b  -Ip 


(40) 


The  transformed  accelerometer  outputs,  ap,  are  then  processed 
through  the  block  labelled  "J"  in  Figure  4  to  obtain  vp,  the  vehicle 
linear  velocity  expressed  in  local  wander  azimuth  coordinates.  Section 
III. 2  will  develop  the  appropriate  level  loop  relationships.  Required 
by  these  computations  are  «  pp  and  u  discussed  in  Sections  III. 3  and 
III. 4  respectively.  The  vertical  channel  additionally  requires  com¬ 
putation  of  2?  and  damping  provided  by  a  barometric  altimeter,  considered 
in  Sections  III. 5  and  III. 6. 
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Once  yP  is  formed,  it  can  be  used  to  derive  the  value  of  the  angular 
rate  u  Ppi  the  block  labelled  <*)  is  delineated  in  Section  III. 3.  For  the 

wander  azimuth  system  to  be  employed  (sometimes  termed  free  azimuth), 
u  Ppz  is  identically  zero. 

The  angular  rate  between  the  inertial  and  wander  azimuth  frames, 
up.  ,  is  then  formed  as 


where  u  ?  is  calculated  as  in  Section  III. 4.  This  is  the  quantity  used 
in  Equation  40  and  the  loop  is  complete. 


A  number  of  these  computations  are  functionally  dependent  upon 
latitude  and  longitude.  A  direct  evaluation  of  these  is  possible  by 
solving  for  a  according  to  the  relation 


to  form  C  p  as 


a  =  -  A  sinL 


cos  a  sin  a  0 

-  sin  a  cos  a  0 

0  0  I 


(42) 


(43) 


which  in  turn  is  used  to  transform  a  ?  to  e  ", 

—  ep  —  ep 


with  x  and  y  components 


W«px  =  *  eo*  L  (44a) 

%y  m  *  L  (44b) 

which  can  be  Integrated  to  evaluate  L  and  However,  the  functional 
relationships  required  are  easily  generated  from  the  elements  of  C  p. 
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Since  this  involves  a  linear  propagation  equation  without  the  Inherent 
problems  of  direct  integration  of  a,  L  and  A,  it  is  more  efficient  to 
use  u  £p  to  update  C  p.  Then  L,  A,  a,  and  the  desired  functions  are 
computed  from  the  direction  cosines. 

In  a  similar  manner,  (tp  +  a),  6,  and  $  are  extracted  from  C  while 
these  values  are  not  used  explicitly  in  the  computational  loop.  Section 
III. 7  considers  the  extraction  of  data  for  presentation  to  either  the 
pilot  or  autopilot. 

2.  VELOCITY  EQUATIONS 

The  equation  that  relates  v?  to  af  is 

VP  =  oP  +  gP  -  (2uP#  +  u>P  )  X  VP 

/ 

This  relation  is  derived  in  the  following  manner  (References  4, 

From  Newtonian  mechanics,  the  second  derivative  of  position,  as 
the  inertial  frame  1,  satisfies  the  equation 

P2  i  =  3.  +  9m  (46) 

where  r  is  the  vehicle  position  vector,  a  is  the  output  of  the  accel¬ 
erometers,  and  2.  m  is  the  mass  attraction  vector.  If  2  is  the  gravity 
vector  composed  of  both  mass  attraction  and  centripetal  components, 

I  ■  2m  -  S|,  x  (  -le  x  (47) 

then  Equation  (46)  can  be  written  as 

P >2  X  =  £  +  ^  +  S',,  X  ("1,  X  i)  (48) 

It  is  desired  to  write  Equation  48  in  terms  of  the  rate  of  change  of 
the  velocity  vector  y  ,  as  seen  from  the  computational  frame  p.  By 


(45) 

8). 

seen  from 
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When  coordlnatlzed  in  the  p  frame,  this  is  the  desired  relation, 
(Equation  45). 
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Writing  Equation  45  In  scalar  form  yields  the  relations  to  be  Imple¬ 
mented  in  the  algorithm: 


•P  p  p  p  p  p 
v  =  a,  +  v.  m.  -  v,  io. 


x 

•  P  . 


“X 

p  ,  p  p 
+  vt  "x 


P  P 

V  U) 

vx  z 


.  p  p  p  p  p  p 

Vz  =  a*  +  g  +  vxKa»J  -  vy<*£ 

where  the  quantities  </,  (A  and  cop  are  defined  as 
x  y  z 


(55a) 

(55b) 

(55c) 


p  p  p 

to,  s  2(0,  +  to. 

*  lex  *P* 

cuP  =  2wP  +  <oP 

V  ley  epy 


<op  =  2toP 
z  laz 


(56a) 

(56b) 

(56c) 


Note  that,  for  the  wander  azimuth  system  being  used,  iappz  Is  zero. 
Also,  the  approximation  that  tj.  is  colinear  with  zp  has  been  made: 

i 

0 
0 
g 


(57) 


This  will  introduce  only  negligible  errors,  on  the  order  of  resolution 
capabilities  of  present  day  instruments. 


Equation  55c 
The  computations 
Section  III. 6  to 


is  the  equation  for  an  unaided  inertial  vertical  channel, 
are  unstable,  and  so  this  equation  will  be  modified  in 
provide  damping  by  means  of  barometric  altimeter  data. 
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3.  ANGULAR  RATE  RELATIONS 

Let  rQ  be  the  equatorial  radius  of  the  earth  and  h  be  the  vehicle 
altitude.  If  a  spherical  earth  were  assumed,  the  angular  rate  relations 
would  simply  be 


p  _ 

llepx 


r0+h 


(58a) 


P  .  I 
"ep>  '  '  r„+h 


(58b) 


However,  It  Is  desired  to  account  for  the  earth's  elliptlcity,  so  the 
relations  become  somewhat  more  complex. 


Consider  the  north-east-down  (n)  frame  first.  With  respect  to  this 
frame,  the  desired  relations  are 

v$  (59a) 

V1  (59b) 

where  rn  is  the  radius  of  curvature  of  the  reference  ellipsoid  in  the 
meridian  plane  at  a  given  point  on  the  surface,  and  rg  is  the  radius  of 
curvature  In  the  plane  orthogonal  to  the  meridian  plane  at  that  same 
point.  Thus,  the  terms  (rn  +  h)  and  (rg  +  h)  are  the  corresponding  radii 
of  curvature  at  altitude  h  (h  defined  along  zn  or  zp  Is  col  Inear  with  both 
rn  and  rg).  The  values  of  rn  and  rg  can  be  shown  to  be  [Reference  3]. 


wenx  = 


re+h 


=*  rn+h 


TqH  -  «2) 

rn  ‘  (I  -  «zsini:L>4'i£ 


e  <I-«Wl>* 

where  c  Is  the  eccentricity  of  the  reference  ellipsoid, 


,z  .2 
'o  ~rP 
,2 


rp  being  defined  as  the  polar  radius. 


(60a) 

(60b) 

(61) 
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The  vehicle  angular  rates  expressed  In  the  n  and  p  frames  are  related 
by 

wcpx  =  cosa  •■wjny  sina  (62a) 

u.epy  =  “en*  sina  +  w£ny  cos  a  (62b) 

and  similarly  the  linear  velocities  are  related  by 

vj  =  yPcosa  +  vPsina  (63a) 

vj= -vxsina  +  vjcosa  (63b) 

Substituting  Equation  63  into  Equation  59,  and  then  substituting  the 
results  into  Equation  62,  yields 


where 


wepx  “  cr  Vy+kv^ 

Ry 


“ep,  *-jj-  vxP-Kv? 
Kx 


(64a) 

(6<tb) 


1 

cos2  a 

,  sin2  a 

Rx  ' 

m+h 

fe+h 

J.  . 

sm2a 

cos  2a 

Ry 

rn+h 

re+n 

(65a) 


(65b) 


K=<r^h  “TgTh1  s,nacosa 


(65c) 


Equations  64  and  65  provide  the  exact  expressions  for  vehicle 
angular  rates  In  terms  of  linear  velocities,  assuming  an  oblate  spheroid 
for  the  earth  (l.e.,  with  elliptical  meridian  planes). 
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First  order  approximations  to  these  equations  can  be  derived  by 
expanding  (rn  +  h)'^  and  (rfi  +  h)'^  and  retaining  only  first  order  terms 

to  obtain  [Reference  3]: 

r4-s  f  (.--a-* «'-!«*  «A->  (sea) 

rn rn  ro  ro 

r7Tn=  i('-?o-^2sin8u)  (66b) 

2 

These  approximations  are  put  into  Equation  65,  and  1/2  c  is 
approximated  by  the  ellipticity  e,  where 

e=_kZ-l£_  (67) 

r0 

yielding  the  first  order  expressions 

i  =  f  I  -  £  — e(l -3cos2Lcos2a  -cos2Lsin2a)j  (68a) 

Kx  fo  t  ro  1 

[,—  ^—e(,-3cos2l-sm2a  -cos2l.cos2a)]  (68b) 

k=  cos2Lsma  cosa  (68c) 


These  evaluations  can  be  expressed  conveniently  in  terms  of  the 


elements  of  the  direction  cosine  matrix  C  jj: 

Rxsf0['-r0-e"-3C»-cia>] 

(69a) 

RyS4  (l_Tb-e(l~3C”~  '«>] 

(69b) 

K  =  CI5C2J 

(69c) 

The  algorithm  to  be  implemented  computes  R,*',  R, ,  and  k  according  to 

a  y 

Equation  69,  and  then  calculates  and  through  Equation  64. 
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4.  EARTH  RATE  COMPONENTS 

In  the  earth  (e)  coordinate  system,  the  earth  rotational  rate  vector 


Thus,  In  wander  azimutn  coordinates,  this  vector  becomes 


<vie  Cia 

P  .  rP  ,  ,e  .  c 

Wie  '  -ie  '  u,e 


5.  GRAVITY  COMPUTATION 

The  standard  approximation  to  the  value  of  gravity  (Reference  3)  is 
given  by: 


g  =  fr  [d  +  t  dj,-5!  J4-C)  + 

«o  v 

+  (c  2-^Cc2+15J2c2-|«‘,-|j2+?J,+C)sin!!L 
-(2+6Jz+C)?  j 


where  G  is  the  gravitational  constant,  M  is  the  mass  of  the  earth,  0, 
and  are  coefficients  of  the  first  two  harmonics  of  the  mass  attraction 

2  3 

potential  function,  and  C  is  equal  to  rQ /(GM).  Substituting  the 
numerical  values  involved  and  using  Cjj  =  -sin  L,  Equation  72  is  put 
into  final  form  as 


9  =  32087437360 

+  OI69944938I5Cjj1 

-  0  30877321095  +  lO'5hQ  ( fl/sec2 )  (73) 

The  subscript  B  on  hg  (measured  in  feet)  denotes  the  fact  that  barometric 
altitude  is  used  to  evaluate  g  in  order  to  stabilize  the  vertical  channel 
computations.  This  will  be  discussed  further  in  the  following  section. 
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6.  VERTICAL  CHANNEL  DAMPING 

Barometric  altimeter  data  is  combined  with  inertial  vertical  channel 
computations  to  damp  and  bound  the  errors  in  the  latter.  A  third  order 
damping  technique  is  implemented  as  in  Figure  5.  The  inertial  system 
input  is  (a£  +  v£  -  v£),  where  a£  is  provided  by  Equation  39  and 

the  other  terms  are  evaluated  as  in  Section  III. 2.  Added  to  this  is  the 
computed  value  of  g.  The  altitude  hg  provided  by  the  barometric  altimeter 
is  subtracted  from  h,  the  inertially  Indicated  altitude,  to  generate  the 
error  signal  for  feedback  (note  that  h  is  positive  up).  The  vertical 
velocity  is  passed  through  the  gain  C4  and  added  to  h  (since  v^  is 
positive  down)  in  order  to  compensate  for  the  inherent  lag  in  the 
barometric  altimeter.  Thus,  typical  values  of  C^  would  range  between 

7 

0.6  and  0.8  seconds.  The  feedback  of  h  through  2o>s  compensates  for 
changes  in  gravity  with  altitude,  and  it  can  be  recognized  as  the  cause 
of  instability  in  the  unaided  inertial  vertical  channel.  Although  a 
simpler  second  order  damping  system  would  result  from  setting  C3  equal 
to  zero,  the  performance  advantage  of  the  more  complex  system  is 
sufficient  to  warrant  its  use. 


From  Figure  5,  the  state  space  model  of  the  vertical  computational 
loop  is 


-(l+C,C„) 

c2c4 

C3C4 


"  c,' 

0 

hB  + 

p  p  p  p  p 

Oj  +  9  +  v,  -  w*  vy 

-C3 

0 

-  *. 

(74) 


Note  that  h  is  defined  to  be  positive  upward  for  convenience,  v£  is 
positive  downward,  and  x  is  the  second  derivative  of  h. 
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The  values  of  C^,  C2,  and  C3  are  set  so  that  the  system  characteristic 
equation  yields  three  poles  at  (-1/t), 

(S  +  l/r)1  =  0  (75> 

where  t  Is  the  desired  vertical  channel  time  constant,  usually  chosen  to 
be  about  200  seconds.  The  gain  values  that  achieve  trfese  system  poles 
are: 


Zu j 

C4r3  +  3r2  +  3C4  r  +  C4 

(76a) 

c,  - 

(l  -  2uif  C4  )  TS 

> 

1 

ro 

{ rs  +  3C4  t2)  +  3r  +  C4 

(76b) 

- 

(|-2<4c2)  rs 

(76c) 

Once  the  appropriate  values  for  t  and  are  chosen,  these  relations 
yield  a  time  invariant  linear  system  equation.  A  discrete-time  updating 
equation  can  then  be  derived  by  means  of  the  state  transition  matrix 
technique.  This  will  be  discussed  more  fully  in  Section  IV. 1. 

The  hg  value  provided  by  the  air  data  system  can  be  calculated  in  the 
standard  manner,  or  by  means  of  the  technique  developed  by  R.L.  Blanchard 
(Reference  1).  Rather  than  calculating  pressure  a  titude  based  on  a 
standard  atmosphere  model  (and  possibly  compensating  later  for  non¬ 
standard  conditions),  this  method  computes  a  dynamically  corrected  alti¬ 
tude  by  numerically  integrating  the  physical  relation  fo/a  column  of  air 
as  a  function  of  pressure,  gravity,  and  absolute  temperature.  The 
algorithm  has  already  been  flight  tested  and  has  demonstrated  very 
accurate  performance.  Letting  TJ(1  and  be  the  absolute  temperature 
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and  pressure  available  from  the  air  data  system,  the  Blanchard  algorithm 
with  trapezoidal  integration  can  be  written  as 

0  =  Wpod 

1  M0  +  Si  *•«*  L  +  92hg 

hB=  h0-  C/g  (0+DOLB)(Pad-  P0LD) 


(77) 


In  the  above  equations,  g0,  g-j ,  and  g2  are  the  values  given  in 
Equation  73,  and  C  equals  the  universal  gas  constant  R  divided  by  twice 
the  molecular  weight  of  the  atmosphere,  2M.  Again  it  is  noted  that  hg, 
rather  than  h,  is  used  to  evaluate  g. 


7.  INFORMATION  EXTRACTION 

The  values  of  latitude  l,  longitude  X,  and  wander  azimuth  a  are 
derived  from  the  six  propagated  elements  of  C  p  as  in  Equations  11,  12, 
and  13,  using  Equation  14  and  the  logic  of  Table  I.  Similarly,  (i|>  +  a), 

9,  and  $  are  computed  from  the  elements  of  C  £  using  Equations  18,  19, 
and  20,  and  the  logic  of  Table  II.  Since  these  values  are  not  required 
in  the  computational  loop  itself,  the  data  extraction  can  be  performed  at 
any  suitable  iteration  rate  without  any  effect  upon  algorithm  performance. 
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SECTION  IV 

ONBOARD  IMPLEMENTATION 


1.  INTEGRATION  TECHNIQUES 

A  number  of  propagation  relations  require  integration  in  this  system 
algorithm.  Oue  to  computer  loading  considerations,  first  order  inte¬ 
gration  techniques  will  be  used  wherever  they  provide  adequate  perfor¬ 
mance.  However,  updating  the  elements  of  C  £  will  necessitate  con¬ 
sideration  of  higher  order  techniques  because  of  the  potentially  high 
angular  rates  involved.  Previously  the  approach  has  been  to  employ  DDA's 
operating  at  very  high  rates,  as  about  10,000  iterations  per  second,  to 
provide  accuracy  with  a  first  order  technique.  In  contrast,  this  program 
uses  a  whole  word  computer  operating  at  a  slower  iteration  rate,  but 
using  higher  order  Integration  methods  to  achieve  the  desired  precision. 

There  are  various  means  of  generating  higher  order  techniques,  such 
as  the  Runge-Kutta  method  and  Taylor  Series.  Runge-Kutta  techniques 
have  been  chosen  because  they  provide  an  accuracy  equivalent  to  that  of 
a  Taylor  Series  solution  of  the  same  order,  while  not  requiring  past 
histories  of  the  dependent  variables  or  high  order  derivative  Information. 

Consider  the  updating  of  the  direction  cosine  matrices  C  £  and  C  £. 

Either  the  four  quaternion  parameters  for  C.  £  or  the  six  direction 
cosines  for  C  £  (three  are  not  propagated)  can  be  arranged  into  a  vector, 

denoted  as  x(t).  Then  an  expression  for  the  time  rate  of  change  of  that 
vector,  *( t),  can  be  written  as 

i(»l  =  L  [*.<»).&>(»)  }  (78) 

For  the  case  of  updating  C  £  using  u  jjb,  the  quaternion  relations 
corresponding  to  Equation  78  are 

A  =  -  uyC  -  u>tD)  (79a) 
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6  !  j  ('“l*  +  <*'xc  -  "y  0) 

(79b) 

C  =  (  Wy  A  -  u),B  -  lUjO) 

(79c) 

D  =  4  (  at,  A  +  oiyB  +  (o,  C) 

(79d) 

where  x.T  * 

(A,  B,  C,  D).  Updating  C  £  with  a  £  entails  the  component 

relations 

®I2  s  "  uy  CS2 

(80a) 

C,3  =  ~  0),  css 

(80b) 

^22  =  wJLC3i 

(80c) 

^25  °  "xC33 

(80d) 

Cj2  =  Wy  C|2  -  0<AC22 

(80e) 

Cjs  =  "y  CI3  "  "x.C2S 

(80f ) 

where 

=  (C,2,  C|j,  C22,  C2j,  Cj2,  C33) 

Let  the  Integration  Interval  be  T  seconds  long. 

Then  a  first  order 

Runge-Kutta  routine  to  solve  for  x(t  +  T)  in  terms  of  x(t)  would  be 

A.O  +  T)  *  5.(1)  +  T  (81) 

This  Is  the  same  as  a  first  order  Taylor  Series  solution  or  a  first  order 
approximation  to  a  transition  matrix  solution  to  a  set  of  linear 
equations. 

A  second  order  Runge-Kutta  algorithm  would  be  solved  as 

y  =  x_(t)  +  T  f [x(l),oj{l)]  (82a) 

x(1  +  T)  =  a_(t)  +  T/2  {f  [i_(l),o>(t>]  +  L[y.to(l  +  T)]}  (82b) 
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Finally,  a  fourth  order  Runge-Kutta  technique  computes  x(t+T) 
according  to  the  following  algorithm: 


a  = 

T  f.[x.(»),4>(t)] 

(83a) 

s  - 

T  i[*(t)  +  |a,a(t  +  T/2)j 

.  (83b) 

i  z 

T  +  ±£.S>(»  +  T/2)] 

(83c) 

§  « 

T  ifitt)  +  Y,  «(»  +  Tl] 

(83d) 

i(t  +  T)  = 

2.(1)  +  i  (s  +  2/3  +  2y  +  8) 

(83e) 

The  current  algorithm  formulation  employs  fourth  order  integration  of 
the  quaternion  parameters  related  to  C  jj  and  first  order  Integration  of 
the  elements  of  C  jj. 

2.  RATE  EXTRACTION 

All  of  the  Integration  techniques  require  a  knowledge  of  the  angular 
rate  w(t).  In  the  case  of  updating  C  is  generated  in  part  by  the 

output  of  the  gyros,  «  as  seen  in  Equation  40.  However,  the  gyro 
outputs  are  in  the  form  of  a  pulse  rate  proportional  to  angular  rate,  and 
thus  a  pulse  counter  yields  incremental  change  in  angular  orientation 
about  the  j-th  axis,  AS ,.  Let  T  be  the  iteration  period  of  the  inte¬ 
gration  routine.  If  A9j(t,  t  +  T)  denotes  the  A6j  corresponding  to  the 
number  of  pulses  counted  between  the  times  t  and  (t  +T),  a  first  order 
rate  extraction  would  be  computed  as 

Wj<t)  =  l/T  t  +  T>  (M) 

This  would  be  adequate  for  a  first  order  integration  method. 

Since  the  fourth  order  algorithm  (Equation  83)  requires  rate 
information  at  the  midpoint  of  the  integration  interval,  u>j(t  +  T/2),  a 
second  order  rate  extraction  is  appropriate.  The  gyro  output  pulse 
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counter  Is  sampled  at  twice  the  iteration  frequency,  every  T/2  seconds. 
Thus  the  measurements  A8j (t ,  t  +  T/2)  and  46 j ( t  +  T/2,  t  +  T)  are  used 
to  calculate  uj(t),  uij(t  +  T/2),  and  uj{t  +  T)  as  required  by  Equation  83. 
Curve  fitting  of  a  second  order  polynomial  to  the  data  produces  the 
following  rate  extraction  relations: 

Ujlt)  =  l/TfsASjtt.t  +  T/2)  -  A6j(t  +  T/2,  t  +  T)]  (85a) 

Wj(t  +  T/2)  =  1/T [  A0j(t,t  +  T/2)  +  ASj(t  +  T/2,  t  +  T)]  (85b) 

Wj(t  +  T)  =  l/T[-Aflj(t,t  +  T/2)  +  3A6j(t  +  T/2,  t  +  T)]  (85c) 

The  generation  of  the  appropriate  values  of  u  ^  to  be  subtracted  from 
the  results  of  Equation  85  will  be  discussed  in  Section  IV.4. 

3.  PROCESSING  RATES 

In  order  to  maintain  accuracy,  the  C  £  matrix  must  be  updated  at  a 
high  frequency.  However,  the  other  computations  need  not  be  propagated 
at  such  a  high  iteration  rate. 

Consequently,  the  algorithm  will  be  partitioned  into  segments  that 
are  Iterated  at  different  rates.  Let  T  be  the  iteration  period  of  the 
computation  of  C  £.  Then  the  gyro  pulse  counters  are  sampled  every  T/2 
seconds,  while  the  accelerometer  pulse  counter  data  is  taken  every  T 
seconds.  The  other  loop  computations  will  be  Iterated  every  NT  seconds, 
where  N  is  an  integer  (for  timing  purposes  In  a  digital  computer,  !i  might 
be  set  most  conveniently  as  some  power  of  two). 

As  has  been  mentioned  previously,  the  information  extraction  (i.e., 
evaluations  of  (^  +  a),  6,  <f>,  L,  A,  and  a)  can  occur  at  a  completely 
different  rate.  For  timing  convenience,  its  period  will  be  assumed  equal 
to  some  Integral  number  times  T,  denoted  as  KT.  An  efficient  technique 
would  be  to  let  K  ■  2N,  and  to  alternate  outputting  [L,  A,  a]  and 
[(iji  +  o),  6,  $]  on  successive  passes  of  the  slower  loop  iterations. 
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4.  FINAL  ALGORITHM 

Based  upon  the  previous  discussions,  the  overall  system  is  partitioned 
as  in  Figure  6.  The  value  at  the  top  left  of  each  segment  is  the  period 
of  that  Iteration  cycle.  Using  this  diagram,  the  interfaces  and  passage 
parameters  between  the  various  blocks  will  be  described  briefly. 

First,  the  T/2  block  provides  48  ^  (t,  t  +  T/2)  and  46  !?b  (t  +  T/2, 
t  +  T)  to  the  rate  extraction  algorithm  for  use  in  the  integration 
Interval  (t,  t  +  T).  These  values  are  read  into  six  memory  registers 
reserved  for  gyro  data,  for  subsequent  use  by  the  T  block. 

The  other  Input  to  the  T  block  is  u  ^p,  a  slowly  varying  parameter 
generated  every  N  iterations  of  the  T  block.  Every  T  seconds,  the 
direction  cosine  matrix  £  £  Is  recomputed,  and  a  new  value  ofu  jp  is 

subsequently  calculated  based  on  the  new  C  £  and  the  currently  available 
value  of  u  ^p. 

The  output  of  the  T  block  to  the  NT  block  is  the  sum  of  the  most 
recent  N  values  of_Ay.p(t,  t  +  T).  Every  N  Iterations  by  the  T  block, 
these  sums  are  sent  to  the  NT  block  to  drive  the  velocity  update  equations. 

Finally,  the  2NT  blocks  are  merely  operations  upon  the  current 
elements  of  the  matrices  C  £  and  C  p. 

The  remainder  of  this  section  presents  the  detailed  algorithm.  All 
equations  are  presented  explicitly  in  their  scalar  form. 

a.  The  Partition  Iterated  Every  T/2  Seconds 

Let  t  be  some  Integral  number  of  periods  T  from  initialization.  The 
T/2  block  provides  the  two  samples  of  the  gyro  output  pulse  counters 
required  every  T  seconds.  When  a  counter  is  read,  its  contents  are 
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assumed  to  be  set  to  zero.  A  flag  is  maintained  (denoted  here  as  IPASS 
but  actually  implemented  as  a  countdown  from  the  computer  clock)  to 
denote  whether  the  current  sample  is  of  the  form 

A)  =  4®|bb<M  +  T/2)  (86a) 

or 

62  =  AeJ>+T/2,.  +T)  {86b) 

If  IPASS  equals  2,  the  accelerometer  outputs  are  read  immediately  after 
the  gyro  outputs,  in  order  to  minimize  the  time  difference  of  the  two 
samples.  Thus,  although  the  accelerometers  are  sampled  every  T  seconds, 
the  measurement  appears  in  the  T/2  portion  of  the  algorithm.  Figure  ? 
portrays  the  T/2  block  calculations. 


b.  The  Partition  Iterated  Every  T  Seconds 


Now  consider  the  T  block.  Figure  8  depicts  the  rate  extraction 
algorithm  that  calculates  u  ^  at  times  t,  (t  +  T/2),  and  (t  +  T)  from 
the  gyro  data  Al_  and  A2.  At  time  (t  +  T),  the  quantities  available  for 
updating  C  £  are  these  three  values  of  <o  ^  and  a  value  of  u  based  on 

the  current  value  of  u  ^  and  the  C  £Tof  the  previous  T  block  iteration. 

The  single  value  of  u  ^  is  subtracted  from  each  of  the  three  ca  ^b  values 
to  approximate  the  three  corresponding  values  of  u  pb-  Noting  that 


T,, 

TI2 

T,S 

T» 

T22 

TZJ 

TS, 

T32 

the  matrix  C  “  can  be  written  as 


T*l 

TSI 

T.Z 

TZZ 

T32 

T1S 

TZS 

t33 

(87) 


(88) 
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Figure  7.  The  T/2  Block 


% 


n 


43 


AFFDL-TR-73-80 


"lb,(,) 

= 
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Figure  8.  Rate  Extraction 
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Using  this  convention,  Figure  9  presents  the  computation  of  u  j  and 
b  h  —  'P 

the  three  values  of  u  |>.  In  the  a  pb  relations,  the  following  variables 

have  been  defined: 


"i  =  £&> 

(89a) 

ml  =  "pb  t»  + 

T/2) 

(89b) 

ml  =  <i>pb  u  + 

T) 

(89c) 

Also,  the  rate  extraction,  <u  *?p  and  u  jjb  blocks  are  not  combined  because 

u  is  used  explicitly  by  alignment  procedures,  and  thus  should  be 
separated  to  prevent  redundant  programming. 

Figure  10  presents  the  C  jj  update,  employing  fourth  order  Runge-Kutta 
integration  of  four  quaternion  parameters.  The  initial  value  of  C  £  is 
available  from  the  alignment  routines,  and  its  elements  can  be  related  to 
the  initial  Euler  angles  and  wander  azimuth  by  Equation  17.  From  the 
initial  elements  of  C  ^  can  be  generated  the  initial  values  of  A,  B,  C, 
and  0  through  Equations  74  and  76.  The  update  entails  calculation  of  the 
small  changes  In  the  quaternion  parameters,  AA,  AB,  AC,  and  AO,  in  single 
precision,  followed  by  a  double  precision  update  to  the  quaternion 
parameters  themselves  and  the  ilements  of  C  Double  precision  is 
required  to  add  or  subtract  quantities  of  widely  varying  magnitude,  or  to 
calculate  the  small  difference  of  large  numbers,  without  excessive 
performance  degradation  due  to  truncation.  The  quaternion  parameters  are 
calculated  and  stored  in  double  precision,  whereas  the  T^  values  are 
computed  with  double  precision  operations  but  are  stored  in  single 
precision.  This  is  because  the  T^  values  are  not  used  in  subsequent 
updating,  but  are  computed  algebraically  from  the  updated  A,  B,  C,  and  D. 
In  fact,  performance  analyses  may  Indicate  that  the  T^  values  can 
adequately  be  calculated  in  single  precision. 
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CALCULATE  • 

=  Tn 

u/P 

Ip* 

+  T*l  "?p,  + 

TS| 

lbp, 

*  T,2 

Ip* 

+  ^"fp,  + 

T32 

Ip* 

*  T» 

<D.P 

IP* 

+  T»"lp,  + 

TS, 

CALCULATE  wPb  : 

"'x  * 

"lb* 

“»  “  <* 

"lb, 

(t)  -  <-!> 

Ip, 

"'*  = 

wb 

ibz 

(»)  -  v* 
Ip* 

"zx  » 

"ibx 

(»  +  T/i)  - 

W1ft 

Ipx 

"2,= 

"lb, 

(t  +  T/E)  - 

J> 

Ip, 

"Zz  = 

"ibz 

(1  +  T/2)  - 

"I* 

Ip* 

0)3  r 
X 

"fbx 

(,  +T)  - 

tub 

Ip* 

W3y  = 

"lb, 

(t  +T)  - 

u,f> 

Ip, 

w3z  = 

"ibx 

U+T)  - 

6»b 

Ip* 

Figure  9.  Calculation  of  w  J  and  u  Jj. 
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Q,  =  T/2  ( wlz-B -wly-C-w}x-D) 
a2  =  T/2  (wlx-C-wlz*A-wlyD) 

Qj  =  T/2  ( CLlly  “A—Wlx*  B-Wl^’O) 

Q4  =  T/2  ( ojlx’A+culy1  B+culz'C) 

Al  =  A  +  ■£  q  j 
Bl  =  8  +^a2 
Cl  :  c  +  ja3 
Di  =  0  +  ^a4 

0,  -  T/2(w2z-8l-u>2yCI-w2xDI) 
0 2  =  T/2  ((d2X'CI“w2z*AI— u/2y*DI) 
@3  -  T/2  (ui2y  AI-oj2x  BI-u2z  DI) 
0A  =  T/2  (ui2x*  Al  +  cj2yBI  +u>2Z‘CI) 
A2  =  A  +  4/9, 

82  =  8  +  £/32 
C2  r  C  +  iP, 

02  =  0  + 

y,  =  T/2(u2jB2-«2yC2-w2xD2) 
Yz  •  T/2(w2x  C2-u>2zA2-u>2yD2> 
Yi  =  T/2(<<i2yA2-w2yB2-w2z  02) 
y«  =  T/2(uj2yA2+w2yB2+w2zC2) 
A3  =  A  +  y, 

B3  =  B+y2 


Figure  10.  C  £  Update 
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C3 

= 

c 

+  >i 

D3 

= 

D 

+  y4 

8, 

- 

T/2 

(w 3j  •  B3  -  <o3y  •  C3  -  <03, 

•  D3) 

8* 

= 

T/2 

(<o3  •  C3  -  <o3  ■  A3  -  <o3 
*  *  y 

■  D3) 

8S 

= 

T/2 

(<o3y  •  A3  -  <o3,  •  B3  -  <o3,  ■ 

•  03) 

84 

= 

T/2 

(<u3  •  A3  +  <o3  •  B3  +  <o3.  • 

C3) 

aa 

1/6 

[a,  +  2(/?,  +  >j)  +  S,] 

AS 

* 

1/6 

h  +  2  (£2  +  r2)+  8j 

Ac 

= 

1/6 

[a,  +  2(/?s  +  ys)+  Sj] 

Ad 

S 

1/6 

K  +  2(^4  +  >;)+  84] 

*  DOUBLE  PRECISION  * 


A  - -  A  +  AA 

B  - -  B  +  AB 

C  —  C  +  Ac 
D  —  D  +  Ar 


T„ 

= 

AZ  -  8Z  -  CZ  +  DZ 

TI2 

2 ( AB  +  CD) 

TI3 

2  (AC  -  BD) 

T2I 

2 ( AB  -  CD) 

T22 

-  AZ  +  BZ  -  CZ  +  D! 

T23 

= 

2  (BC  +  AD) 

T3» 

2  ( AC  +  BD) 

/ 

T32 

2 (BC  -  AD) 

T33 

= 

-  AZ  -  BZ  +  CZ  +  DZ 
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Depicted  in  Figure  11  is  the  transformation  of  the  accelerometer  data 
into  the  wander  azimuth  coordinate  frame.  The  sum  registers  are 
initialized  at  zero,  and  at  the  end  of  each  T  block  loop,  the  components 
of  iv  p(t,  t  +  T)  are  added  to  them;  when  these  sums  are  used  by  the  NT 
block,  the  register  contents  are  reset  to  zero. 

c.  The  Partition  Iterated  Every  NT  Seconds 

Now  the  NT  block  calculations  will  be  delineated.  The  unaided 
inertial  system  velocity  updating  equations  were  presented  as  Equations 
55  and  56  of  Section  III. 2.  Further,  the  vertical  channel  damping  was 
described  by  means  of  Equation  74,  using  Equation  76  to  define  numerical 
values  Involved,  and  Equation  77  to  compute  hg.  From  Equation  74  can  be 
derived  a  discrete-time  update  by  using  a  first  order  Runge-Kutta 
(transition  matrix  approximation)  method.  This  is  modified  slightly  to 
express  the  result  in  terms  of  Avp(t,  t  +  NT),  as  available  from  the  sum 
registers  oi  the  T  block.  By  defining 

0..  =  -NTC, 

<#>12=  -tl+C,C4)NT 
d*2i=  (Cj-2wt)NT 
$22  =  NTCjC^ 

<#>23  =  NT 

31  =ntc3 

i)>m  =  NTCjC4 

2 

where  the  C(  values  are  established  in  Section  III. 6  and  ws  is  the 

square  of  Schuler  frequency,  equal  to  1.536217230  x  10'6rad2/sec2,  the 
■J)  block  algorithm  can  be  developed  as  in  Figure  12.  First  the  air  data 
system  sampled.  Then  the  change  In  vp  is  calculated  in  single 
precision  and  added  to  the  previous  value  of  vp  in  double  precision 
("D.P."  means  double  precision).  Similar  separation  of  tingle  and  double 
precision  is  maintained  in  the  vertical  loop  calculation  of  altitude  as 
well.  Once  the  contents  of  the  T  block  sum  registers  are  used,  these 
registers  are  zeroed  for  the  next  iteration.  The  values  of  g,  a  pp  and 
u  ?e  used  in  this  algoritim  are  available  from  the  previous  NT  block 
Iteration. 


b,.=  NTC, 
bj  =-NTC2 
b,=-NTC, 


(90) 
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T„  Avb  +  ti2  Avb  +  T„  Avb 
t2I  Avb  +  r22Avb  +  T2SAvb 


Figure  11.  Computation  of  ivp 
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READ  T0(JtPad 
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2aip 

l*« 

+ 

"Spx 

"5 

-5 

s 

2<<»P 
.  '•» 
2uf 
l«z 

+ 

•py 

Av,P 

= 

2X  + 

NT 

(“! 

-  «p 

v.P) 

Avyp 

= 

V 

NT 

W 

--5 

V/) 

AV,P 

- 

*z  + 

NT 

-  "  X 

vyp  + 

= 

0 

*7 

= 

0 

< 

0 

< 

VN 

AV,P 

Vy  - —  vyp  +  Avp 

0  ■  Wpod 


"•  -  hB  "  (C/«)(°  +  °0L0)  (P0d  "  POLD)  D  P- 


POLD  =  P0d 


D  =  D 
OLD 


y,  =  4>u  h  +  <£,2  vz  +  b,  bB 

» 2  •  *21  h  +  *22  VP  +  *23  X  +  b*  "B  +  ^VP 

y  =  <#>„  H  +  VP  +  b,  hB 


Figure  12.  The^  Blrck 


51 


AFFDL-TR-73-80 


i 


i 


The  Initial  conditions  for  this  Int* 

(ration  are 

h8 

s 

^0 

(+  UP) 

P 

s 

OLO 

0 

D 

s 

T/P 

OLD 

0  0 

h 

s 

h0  INERTIAL 

t+  UP) 

v,P 

s 

VZ0  INERTIAL 

(  +  DOWN) 

0 

vp 

Vo  INERTIAL 
vyO  INERTIAL 

where  hQ  Is  the  altitude  used  to  Initialize  the  altimeter,  and  the  sub¬ 
script  INERTIAL  refers  to  the  Initial  conditions  in  the  inertial  system. 
For  ground  alignment,  the  known  altitude  of  the  runway  would  be  set  Into 
the  air  data  and  Inertial  systems,  with  velocities  and  accelerations  zero 

If  he  Is  to  be  read  directly  from  the  air  data  system,  the  first 
four  statements  after  vp  ♦  vp  +  4vp  can  be  removed.  By  replacing  the 

statements  after  vp  *  vp  +  Avp  with  the  relation  vp  *  vp  +  ovp  , 
an  unaided  Inertial  system  is  obtained. 

The  (i)  segment,  depicted  In  Figure  13,  computes  u  from  vp  using 
the  Cp  elements  generated  in  the  previous  NT  block  Iteration.  In  these 
relations  the  constants  are  the  reciprocal  of  earth  equatorial  radius 
and  elllptlcity: 


_L  B  _ I _ 

f0  20,926,486  ft. 


=  4.778632707  *  10'*  ft'1 


(92) 


a 


| 


<  = 


i 

297.00 


s  3.367003367  x  I0“S 


(93) 


V! 


52 


AFFDL-TR-73-80 


wp 
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•py 
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ACjj  =  NT(uiJp>  C„  -  <uSpxejj) 
»  DOUBLE  PRECISION  • 


12  **“ 

CI2 

4- 

ACIZ 

13 
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23 
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+ 

AC  2, 

32  “ 

CS2 

+ 

AC„ 

33 

C33 

+ 

Ac,, 

Figure  13.  u  end  C  p  Blocks 
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Figure  13  also  depicts  the  first  order  Integration  of  C  P,  In  which 
the  changes  to  the  six  direction  cosines  are  computed  in  single  precision 
and  then  added  to  the  whole  values  in  double  precision.  The  values 
thus  are  stored  In  double  precision,  with  the  most  significant  half  used 
In  subsequent  single  precision  computations. 

The  remainder  of  the  NT  block  calculations  are  shown  in  Figure  14. 

The  earth  rate  vector  w  J  Is  generated  with  the  newly  updated  elements 
of  C  P  and  the  constant  u1e>  7.2921160  x  10"5rad/sec.  Having  generated 

u  gp  and  u  !Je>  u  ^  is  then  formed  as  their  sum,  to  be  used  as  an  output 

to  the  T  block.  Finally,  g  is  calculated  with  the  current  value  of  C33 
and  the  value  of  hg  from  therP  block. 

d.  Data  Extraction 

As  presently  programmed,  data  is  extracted  from  C  £  on  odd  numbered 

Iterations  of  the  NT  block,  and  from  C  j|  on  even  numbered  iterations. 

THe  C  £  extraction  is  shown  In  Figure  16,  and  that  from  C  £  in  Figure  16. 
As  Is  noted  In  these  figures,  doub’e  precision  may  be  required  in 
evaluating  the  sin"'  and  tan"'  functions  to  provide  adequate  resolution. 


54 


AFF0L-TR-73-80 


CALCULATE 


".Pe»*"iecB 

“Pey  =wiac23 
"Pez5  "iec3! 


CALCULATE  a>fp: 


(d 
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Id 
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IpX  = 

P  _ 
ipy- 

P  . 
rpy- 


wfexH 

w  fey  ** 
p 

wi« 


"epx 

uepy 


CALCULATE  g'. 


g  =  32,087437360 

+  O.I69944938I5  Cj,2 
-  0  30877321095  XI0'5hB 

Figure  14.  Calculation  of  a  ^g,  w  ^  and  £ 
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L  -  sin  1 1 —  C33 ) 

Cjl  =  C23CI2-C22C|3 

X  =  Ian  l(C32/C3l) 

IF  (Cj,=  +  and X  =+),  X  —X  -180° 

IF  (Cj,  =  +  ondX  =  -),  X  -X  +  180“ 
a  *  tan  '(C^/C^) 

IF  (Cl5=  - ),  a  — a  +180 

IF  { C ,3  —  +  onda  =  -),  a  —a  +360 

sin”1  ( ■)  and  ton'1!  )  MAY  REQUIRE 
DOUBLE  PRECISION 

Figure  15.  2Nt  Block  Associated  with  C  £ 
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i p  =  ton  *  ( T 32 /T 33) 

IF  (Tj3=-ond  (#>  = +),  <#>—<#>- 180" 

IF  (Tjj  =  -ond^  =  -),^--</>  +180' 

<pj  -  ton'lTji/Tji) 

IF  (Tm=  ->,  "hr-  V'T  +  180' 

IF  (T,  1  =  +ond  ipj  -  -),if»T  *-i(/t  +  360' 
ip  -  ipj  -  a 

IF  (*=-),  \p  —  \p  +  360® 

stn”'(-)  ond  ton''<)  MAY  REQUIRE 
DOUBLE  PRECISION 


Figure  16.  2NT  81ock  Associated  with  C 
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SECTION  V 
USE  OF  ALGORITHM 

This  report  has  developed  the  algorithm  to  derive  attitude  and 
position  information  from  the  outputs  of  strapdown  gyros  and  accelero¬ 
meters,  By  choosing  appropriate  values  for  the  fast  loop  period  T,  the 
multiplicative  factor  N  to  obtain  the  slow  rate,  and  the  integration 
order  to  be  employed  for  the  various  updates,  a  specified  computational 
accuracy  can  be  attained  for  any  expected  vehicle  environment.  Performance 
analyses  can  determine  these  parameter  values  for  each  application. 

The  specific  program  for  which  this  algorithm  was  developed  envisions 
an  angular  rate  environment  in  excess  of  one  radian  per  second,  and  so  a 
fourth  order  integration  of  C  £  was  chosen.  With  first  order  integrations 
used  for  the  other  propagations,  the  values  of  T  =  0.025  sec  and  (1  =  4 
have  been  projected  as  yielding  adequate  performance.  However,  with 
other  choices  of  these  parameters,  and  specification  of  desired  vertical 
channel  time  constant  t  and  altimeter  lag  C^,  the  algorithm  can  be 
adapted  to  any  strapdown  application. 

To  achieve  desirable  system  reliability,  redundant  strapdown  sensors 
will  be  used  in  many  prospective  systems.  The  basic  algorithm  remains 
unchanged  for  this  application.  Logic  network.  would  be  include,  in  the 
system  to  perform  failure  detection,  faulty  sensor  isolation,  and  sensor 
recalibration  and/or  system  reconfiguration.  Thus,  the  sensor  outputs 
are  processed  to  attain  a  best  representation  of  the  quantities  46  bb  and 
4vb  coordinatized  along  the  three  body  axes,  and  these  are  then  used  as 
inputs  to  the  algorithm. 

Thus,  the  algorithm  is  flexible  enough  to  be  compatible  with  a  variety 
of  different  vehicle,  mission,  and  strapdown  sensor  package  configurations. 
With  a  moderate  increase  in  computer  loading,  the  advantages  that  a  strap- 
down  system  bears  over  a  gimballed  platform  can  be  fully  exploited  in 
future  aerospace  vehicles. 
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